/*
Olympus Heavy Industries - Tricorder version 5 fw O.9
https://ohicosplay.tumblr.com/
Based on loads of code, mostly not mine.
Uses ~30% memory space with Feather M0
*/

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <string.h>
#include "SPI.h"
#include "Adafruit_GFX.h"
#include "quaternionFilters.h" //for 9DOF

#include <Statistic.h>

#include <SD.h> //Storage (Feather uSD Reader)

#include <Adafruit_CAP1188.h> //Touch (buttons)
#include <Adafruit_BME280.h> //ATM (temp, bar pressure, humidity)
#include <Adafruit_TSL2561_U.h> //LUX (single sensor)
#include <Adafruit_MLX90614.h> //IR TEMP
#include "RTClib.h" //DATA (RealTimeClock)
#include "Adafruit_ILI9341.h" //TFT 320x240
#include "Adafruit_SI1145.h" //UV_IR_VIS
#include "Adafruit_VEML6070.h" //UV_REAL
#include "Adafruit_TCS34725.h" //COLOR
#include <Adafruit_AMG88xx.h> //GRIDEYE (ADAFruit)
#include "RadiationWatch.h" //RAD
#include "MPU9250.h" //9DOF

#include "FastLED.h" //PIXELS

#define TCAADDR 0x70 //i2C Multiplexer

FASTLED_USING_NAMESPACE;
RTC_DS3231 rtc; //DATA clock
Adafruit_BME280 bme; // ATM
Adafruit_TSL2561_Unified tsl = Adafruit_TSL2561_Unified(TSL2561_ADDR_HIGH, 12345); //LUX
Adafruit_SI1145 uv = Adafruit_SI1145(); //UV_IR_VIS
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X); //COLOR
Adafruit_VEML6070 uv2 = Adafruit_VEML6070(); //UV_REAL
Adafruit_MLX90614 mlx = Adafruit_MLX90614(); //IR TEMP
Adafruit_AMG88xx amg; //GRIDEye
RadiationWatch radiationWatch; //RAD
MPU9250 myIMU; //9DOF

const byte interruptPin = 5;
int timerCounter = 0;
boolean inReset = false;

#define TFT_DC A0
#define TFT_CS 6 //TFT LCD

#define SEALEVELPRESSURE_HPA (1031.494) //ATM

byte gammatable[256];

#define AHRS true         // Set to false for basic data read, for 9DOF

//------------------------------GridEYE----------------------
//low range of the sensor (this will be blue on the screen) IN DEG CELCIUS
#define MINTEMP 20

//high range of the sensor (this will be red on the screen) IN DEG CELCIUS
#define MAXTEMP 40

//the colors we will be using
const uint16_t camColors[] = {0x480F,
0x400F,0x400F,0x400F,0x4010,0x3810,0x3810,0x3810,0x3810,0x3010,0x3010,
0x3010,0x2810,0x2810,0x2810,0x2810,0x2010,0x2010,0x2010,0x1810,0x1810,
0x1811,0x1811,0x1011,0x1011,0x1011,0x0811,0x0811,0x0811,0x0011,0x0011,
0x0011,0x0011,0x0011,0x0031,0x0031,0x0051,0x0072,0x0072,0x0092,0x00B2,
0x00B2,0x00D2,0x00F2,0x00F2,0x0112,0x0132,0x0152,0x0152,0x0172,0x0192,
0x0192,0x01B2,0x01D2,0x01F3,0x01F3,0x0213,0x0233,0x0253,0x0253,0x0273,
0x0293,0x02B3,0x02D3,0x02D3,0x02F3,0x0313,0x0333,0x0333,0x0353,0x0373,
0x0394,0x03B4,0x03D4,0x03D4,0x03F4,0x0414,0x0434,0x0454,0x0474,0x0474,
0x0494,0x04B4,0x04D4,0x04F4,0x0514,0x0534,0x0534,0x0554,0x0554,0x0574,
0x0574,0x0573,0x0573,0x0573,0x0572,0x0572,0x0572,0x0571,0x0591,0x0591,
0x0590,0x0590,0x058F,0x058F,0x058F,0x058E,0x05AE,0x05AE,0x05AD,0x05AD,
0x05AD,0x05AC,0x05AC,0x05AB,0x05CB,0x05CB,0x05CA,0x05CA,0x05CA,0x05C9,
0x05C9,0x05C8,0x05E8,0x05E8,0x05E7,0x05E7,0x05E6,0x05E6,0x05E6,0x05E5,
0x05E5,0x0604,0x0604,0x0604,0x0603,0x0603,0x0602,0x0602,0x0601,0x0621,
0x0621,0x0620,0x0620,0x0620,0x0620,0x0E20,0x0E20,0x0E40,0x1640,0x1640,
0x1E40,0x1E40,0x2640,0x2640,0x2E40,0x2E60,0x3660,0x3660,0x3E60,0x3E60,
0x3E60,0x4660,0x4660,0x4E60,0x4E80,0x5680,0x5680,0x5E80,0x5E80,0x6680,
0x6680,0x6E80,0x6EA0,0x76A0,0x76A0,0x7EA0,0x7EA0,0x86A0,0x86A0,0x8EA0,
0x8EC0,0x96C0,0x96C0,0x9EC0,0x9EC0,0xA6C0,0xAEC0,0xAEC0,0xB6E0,0xB6E0,
0xBEE0,0xBEE0,0xC6E0,0xC6E0,0xCEE0,0xCEE0,0xD6E0,0xD700,0xDF00,0xDEE0,
0xDEC0,0xDEA0,0xDE80,0xDE80,0xE660,0xE640,0xE620,0xE600,0xE5E0,0xE5C0,
0xE5A0,0xE580,0xE560,0xE540,0xE520,0xE500,0xE4E0,0xE4C0,0xE4A0,0xE480,
0xE460,0xEC40,0xEC20,0xEC00,0xEBE0,0xEBC0,0xEBA0,0xEB80,0xEB60,0xEB40,
0xEB20,0xEB00,0xEAE0,0xEAC0,0xEAA0,0xEA80,0xEA60,0xEA40,0xF220,0xF200,
0xF1E0,0xF1C0,0xF1A0,0xF180,0xF160,0xF140,0xF100,0xF0E0,0xF0C0,0xF0A0,
0xF080,0xF060,0xF040,0xF020,0xF800,};

unsigned long delayTime;
float pixels[AMG88xx_PIXEL_ARRAY_SIZE];
uint16_t displayPixelWidth, displayPixelHeight;

//------------------------------SC Card----------------------
Sd2Card card;
SdVolume volume;
SdFile root;
const int chipSelect = 4; //uSD Storage

//--------------------------------Gas Sensor-----------------
#define CH_VALUE_NH3 1
#define CH_VALUE_CO 2
#define CH_VALUE_NO2 3
#define CMD_CONTROL_LED 10

unsigned char dta_test[20];
unsigned int readChAdcValue(int ch);
unsigned int adcValueR0_NH3_Buf;
unsigned int adcValueR0_CO_Buf;
unsigned int adcValueR0_NO2_Buf;
    
uint16_t res0[3];       //sensors res0
uint16_t res[3];        //sensors res

enum{CO, NO2, NH3, C3H8, C4H10, CH4, H2, C2H5OH};

inline unsigned int get_addr_dta(unsigned char addr_reg);
inline unsigned int get_addr_dta(unsigned char addr_reg, unsigned char __dta);
inline void write_i2c(unsigned char addr, unsigned char *dta, unsigned char dta_len);
//-----------------------------------------------------------------

#define NUM_LEDS 1
#define DATA_PIN A2 
#define LED_TYPE    WS2811
#define COLOR_ORDER GRB
#define NUM_LEDS    2

#define VBATPIN A7 //Batt Sense
const int PIN_TONE = A4;

char daysOfTheWeek[7][12] = {"Sunday", "Monday", "Tuesday", "Wednesday", "Thursday", "Friday", "Saturday"};
int RTCFail = 0;
int ScanMode = 1;
int ScanLight[3];
int EnterCount = 0;
int ClearData = 1;
int BattPercent;
int AlarmMode;
int MagRead;
int TempRead;
int TempRead2;
int TempAverage;
int pixel[64];
int pixelX = 0;
int pixelY = 0;
int gridStartX = 245;
int gridStartY = 115;
int pixelXend = 8;
int pixelYend = 8;
float PixelTemp;
int i2cScan = 0;
int DevCount = 25;
int OldScanMode;
int AltMode = 0;
int GridCen;
int GridCenA;
int GridCenB;
int GridCenC;
int GridCenD;
int GridAve;
int ButtonAct = 0;
int nineDOFstart = 0;
int RadPulse = 0;
int RadStr;
int RadError;

Statistic myStats;

int pinSpeaker = A4;

Adafruit_CAP1188 cap = Adafruit_CAP1188(); //Touch
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC); //TFT LCD


CRGB leds[NUM_LEDS]; //PIXELS

#if defined(ARDUINO_SAMD_ZERO) && defined(SERIAL_PORT_USBVIRTUAL)
  #define Serial SERIAL_PORT_USBVIRTUAL
#endif


//---------------------------------------------VOID SETUP----------------------------------------------------------------------------------------------------
void setup() {
  //-------------SD Startup-----------------------------
  if (!SD.begin(chipSelect)) {Serial.println("Card failed, or not present");}
  Serial.println("card initialized.");
  String dataString = ""; //for SD card logging, make a string for assembling the data to log
  //----------------------------------------------------
  dataString += "OHI Tricorder v5 - Startup...";
  pinMode(A1, OUTPUT); //PCB Green led (heatbeat)
  pinMode(19, OUTPUT); //Color Sensor LED Control
  digitalWrite(19, LOW);//Color Sensor LED Control (low=off)
  
  pinMode(pinSpeaker, OUTPUT);
  
  tft.begin();
  Serial.begin(115200);
  playTone(1700, 150);
    
  myStats.clear(); //explicitly start stats clean
  
  FastLED.addLeds<LED_TYPE,DATA_PIN,COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
  leds[0] = CRGB(75,30,0); 
  FastLED.show(); //PIXEL Start

  tsl.setGain(TSL2561_GAIN_16X);    //LUX Start
  tsl.setIntegrationTime(TSL2561_INTEGRATIONTIME_13MS);
  
  tft.fillScreen(ILI9341_BLACK); //TFT LCD Start
  DrawFrame();  //DrawDate();
  
  //-----------CAP--------------------
  cap.begin(0x28);
  cap.writeRegister(0x2A, 0x80);  // 0x2A default 0x80 use 0x41  — Set multiple touches back to off
  //cap.writeRegister(0x41, 0x39);  // 0x41 default 0x39 use 0x41  — Set "speed up" setting back to off
  //cap.writeRegister(0x72, 0x00);  // 0x72 default 0x00  — Sets LED links back to off (default)
  cap.writeRegister(0x44, 0x41);  // 0x44 default 0x40 use 0x41  — Set interrupt on press but not release
  cap.writeRegister(0x28, 0x00);  // 0x28 default 0xFF use 0x00  — Turn off interrupt repeat on button hold

  
  //-------------setup for color sense
  for (int i=0; i<256; i++) {
    float x = i;
    x /= 255;
    x = pow(x, 2.5);
    x *= 255;
    gammatable[i] = x;
  }

  uv2.begin(VEML6070_1_T);
  
  pinMode(5, INPUT_PULLDOWN); //TouchInterrupt

  playTone(1000, 50);

  mlx.begin();  
  //---------------------GRIDEye--------------------------------------
  tcaselect(0); //i2c multiplexer set to GRIDEYE
  amg.begin(); //GRIDEye
  displayPixelWidth = tft.width() / 8;
  displayPixelHeight = tft.height() / 8;
  bool status;
  //------------------------------------------------------------------

  leds[0] = CRGB(75,15,18); 
  FastLED.show(); //PIXEL Start
  //---------------------Startup checks-------------------------------
  tcaselect(2); //i2c multiplexer set to BME280-9DOF
  if(!bme.begin()){Serial.println("No BME280 (ATM) found");}
  if(!tsl.begin()){Serial.println("No TSL2561 (LUX) found");}
  if(!uv.begin()){Serial.println("No Si1145 (UV,IR,VIS) found");}
  if(!tcs.begin()){Serial.println("No TCS34725 (COLOR) found");}
  if(!mlx.begin()){Serial.println("No MLX90614 (IR-Temp) found");}
  if (!rtc.begin()) {Serial.println("No RTC Found");}
  
  playTone(1000, 50);  
  radiationWatch.setup(); radiationWatch.registerRadiationCallback(&onRadiation); radiationWatch.registerNoiseCallback(&onNoise);
  powerOn();ledOff(); //Gas Sensor

  leds[0] = CRGB(25,15,65); 
  FastLED.show(); //PIXEL Start
  
  leds[0] = CRGB(0,75,0); 
  FastLED.show();

  int pixel[64]; //GRIDEYE
  
  attachInterrupt(5, ButtonPress, FALLING);

  dataString += "Complete!";
    //-------------------------SD Write------------------------------------
  //SD Logfile check and dump
  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  File dataFile = SD.open("datalog.txt", FILE_WRITE);
  //Serial.println("SD File Opened");

  // if the file is available, write to it:
  if (dataFile) {
    dataFile.println(dataString);
    dataFile.close();
    // print to the serial port too:
    Serial.println(dataString);
  }
    // if the file isn't open, pop up an error:
  else {
    Serial.println("error opening datalog.txt");
  }
  //----------------------------------------------------------------------
  
  playTone(1450, 100); //Startup Complete!

}


//---------------------------------------------VOID LOOP-----------------------------------------------------------------------------------------------------
void loop() {
  //Serial.print("Loop-");
  EnterCount = 0;
  String dataString = ""; //for SD card logging, make a string for assembling the data to log

  /* -----------------------------DEBUG
  tft.setCursor(15,50);
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
  tft.print(ScanMode);tft.print("");tft.print(OldScanMode);tft.print(AltMode);
  */

  DrawDate(); //draw time to UI frame
  
  tcaselect(1);DateTime now = rtc.now(); //Save timestamp to SD buffer
  int date1 = (int) now.year(); dataString += date1; dataString += "/"; int date2 = (int) now.month(); dataString += date2; dataString += "/"; int date3 = (int) now.day(); dataString += date3; dataString += ", ";
  int time1 = (int) now.hour(); dataString += time1; dataString += ":"; int time2 = (int) now.minute(); dataString += time2; dataString += ":"; int time3 = (int) now.second(); dataString += time3; dataString += ", ";
  
  DrawBatt();
  
  uint16_t clear, red, green, blue; //color
  
  uint8_t touched = cap.touched();
  for (uint8_t i=0; i<8; i++) {
    if (touched & (1 << i)) {
      ScanMode = i;
      EnterCount = 1;
    }
  }
  if (ClearData == 1){BlankDataReadout();}
  
//--------------------------------------------LUM MODE-------------------------------------------------------------------------------------------------------
  if (ScanMode == 5){
    dataString += "LUM: ";
    sensors_event_t event;
    tsl.getEvent(&event);
    
    digitalWrite(19, HIGH);//Color Sensor LED Control (low=off)

    tcs.getRawData(&red, &green, &blue, &clear);

    float UVindex = uv.readUV();
    UVindex /= 100.0;  
      
    tft.setCursor(40,170);
    tft.setTextSize(1);
    tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      
    if (event.light) //------------------------------------LUX ALARM---------------------------------
    {//tft.print(event.light); tft.println(" lux    ");Serial.print(event.light); Serial.println(" lux");
    //tft.setTextSize(2);tft.setCursor(25,120);tft.setTextColor(ILI9341_BLACK, ILI9341_BLACK);tft.print("                   ");
    }
    else
      {tft.print("LUX Sensor overload  ");
      Alarm();
      tft.setTextSize(2);tft.setCursor(25,107);tft.setTextColor(ILI9341_BLACK, ILI9341_RED);tft.print("LUX Sense Overload");
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.setTextSize(1);
    }

    tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    tft.setTextSize(2);
    tft.setCursor(130,115);tft.print(event.light);tft.print("   ");
    tft.setCursor(130,135);tft.print(uv.readVisible());tft.print("   ");
    tft.setCursor(130,155);tft.print(uv.readIR());tft.print("   ");
    tft.setCursor(130,175);tft.print(uv2.readUV());tft.print("   ");
    tft.setCursor(130,195);tft.print(UVindex);tft.print("   ");
    //tft.setCursor(155,215);tft.print(
      
    
    if (uv2.readUV() >= 2000)//------------------------------------UV ALARM---------------------------------
    {
      Alarm();
      tft.setTextSize(1);tft.setCursor(25,64);tft.setTextColor(ILI9341_YELLOW, ILI9341_RED);tft.print("HIGH   UV   Reading");
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);tft.setTextSize(1);
    }

    if (uv.readIR() >= 4000)//------------------------------------IR ALARM---------------------------------
    {
      Alarm();
      tft.setTextSize(1);tft.setCursor(25,73);tft.setTextColor(ILI9341_YELLOW, ILI9341_RED);tft.print("HIGH   IR   Reading");
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);tft.setTextSize(1);
    }

    //------------------color sense
    uint32_t sum = clear;
    sum += green;
    sum += blue;
    sum += clear;
    float r, g, b;
    r = red; r /= sum;
    g = green; g /= sum;
    b = blue; b /= sum;
    r *= 256; g *= 256; b *= 256;
    if(r==255){r=0;}
    if(b==255){b=0;}
    if(g==255){g=0;}

    int RedSense = (255-(gammatable[(int)r*5]));
    int GreenSense = (255-(gammatable[(int)g*5]));
    int BlueSense = (255-(gammatable[(int)b*5]));


    leds[0] = CRGB(gammatable[(int)r*2], gammatable[(int)g*2], gammatable[(int)b*2]); FastLED.show(); //COLOR pixel

    byte R,G,B; R = r; G = g; B = b;
    int RGBval = (((R*3) & 0xF8) << 8) | (((G*4) & 0xFC) << 3) | (((B*4) & 0xF8) >> 3);
 
    tft.fillRect((gridStartX), (gridStartY), 63, 65, RGBval);
    tft.setCursor(240,182);tft.setTextSize(1);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    tft.print("Col (R,B,G):");
    tft.setCursor(240,195);tft.print(255-(gammatable[(int)r*5]));tft.print(", ");tft.print(255-(gammatable[(int)g*5]));tft.print(",");tft.print(255-(gammatable[(int)b*5]));tft.print("  ");

          //-------------------------------------------SD Logging LUM---------------------------------------
        int sensor1 = event.light; dataString += "Avg Luminosity: ";dataString += String(sensor1);dataString += " Lux, ";
        int sensor2 = (int) uv.readVisible(); dataString += " Near UV: ";dataString += String(sensor2); dataString += " , ";
        int sensor3 = bme.readHumidity(); dataString += " Near IR: ";dataString += String(sensor3); dataString += " , ";
        int sensor4 = mlx.readObjectTempF(); dataString += " UV Index: ";dataString += String(sensor4); dataString += " , ";
        int sensor5 = (255-(gammatable[(int)r*5])); dataString += String(sensor5); dataString += ",";
        int sensor6 = (255-(gammatable[(int)g*5])); dataString += String(sensor6); dataString += ",";
        int sensor7 = (255-(gammatable[(int)b*5])); dataString += String(sensor7); dataString += ",";
        dataString += "RGB, ";
    }
      
//--------------------------------------------ATM MODE-------------------------------------------------------------------------------------------------------    
    else if (ScanMode == 1)
    {
      dataString += "ATM: ";
      nineDOFcheck (); //i2c multiplexer set to BME280-9DOF and query 9DOF
      tft.setCursor(45,195);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      int tempavg1; tempavg1 == (bme.readTemperature()); TempRead = (bme.readTemperature());
      int tempavg2; tempavg2 == (myIMU.readTempData()/100); TempRead = (TempRead + (myIMU.readTempData()/100));
      tcaselect(0); //i2c multiplexer set to GRIDEYE
      int tempavg3; tempavg3 == (mlx.readAmbientTempC()); TempRead = (TempRead + mlx.readAmbientTempC());
      int tempavg4; tempavg4 == (amg.readThermistor()); TempRead = (TempRead + amg.readThermistor());

      TempRead2 = ((TempRead/4) * 9.0)/ 5.0 + 32.0; // Convert Celcius to Fahrenheit

      //---------------------------GRIDEye Adafruit---------------------------------
      //center pixels: 28, 29, 36, 37
      
      amg.readPixels(pixels);

      for(int i=0; i<AMG88xx_PIXEL_ARRAY_SIZE; i++){
        
        GridCen = (GridCen + 1);
        if (GridCen == 28){GridCenA = (int) pixels[i-1];} //checks center 4 pixels, converts to INT and averages
        if (GridCen == 29){GridCenB = (int) pixels[i-1];}
        if (GridCen == 36){GridCenC = (int) pixels[i-1];}
        if (GridCen == 37){GridCenD = (int) pixels[i-1];}
        
        uint8_t colorIndex = map(pixels[i], MINTEMP, MAXTEMP, 0, 255);
        colorIndex = constrain(colorIndex, 0, 255);

        //draw the pixels!
        tft.fillRect((pixelX+gridStartX),(pixelY+gridStartY), 7, 7, camColors[colorIndex]);

        pixelX = (pixelX+8);
        if (pixelX >= 64){pixelX = 0; pixelY = pixelY+8;}
        if (pixelY >= 64){pixelX = 0; pixelY = 0;}
      }
      GridAve = ((GridCenA + GridCenB + GridCenC + GridCenD)/4);
      GridCen = 0;
      //----------------------------------------------------------------------------

      tft.setTextSize(2);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);

      GridAve = (GridAve * 9.0)/ 5.0 + 32.0;
      tft.setCursor(138,95);tft.print((GridAve + mlx.readObjectTempF())/2);tft.print(" *F "); //averages the center of the GRIDEYE readings with the target ir sensor
      tft.setCursor(138,115);tft.print(mlx.readObjectTempF());tft.print(" *F"); //MLX Directed target temp
      tft.setCursor(138,135);tft.print(TempRead2);tft.print(" *F "); //average of all inernal thermometers
      tcaselect(2); //i2c multiplexer set to BME280
      tft.setCursor(138,155);tft.print(bme.readPressure() / 100.0F,0); tft.print(" hPa");
      tft.setCursor(138,175);tft.print(bme.readHumidity(),0); tft.print(" %rH");

      tft.setCursor(30,210);tft.setTextSize(1);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      //-------------------------------------------SD Logging ATM---------------------------------------
        int sensor1 = TempRead2; dataString += "Avg Temp: ";dataString += String(sensor1);dataString += " *f, ";
        int sensor2 = (bme.readPressure()/10); dataString += " ATM Pressure: ";dataString += String(sensor2); dataString += " hPa(*10), ";
        int sensor3 = bme.readHumidity(); dataString += " ATM humidity: ";dataString += String(sensor3); dataString += " %rH, ";
        int sensor4 = mlx.readObjectTempF(); dataString += " Target: ";dataString += String(sensor4); dataString += " *f, ";
        int sensor5 = GridAve; dataString += " GridCen: ";dataString += String(sensor5); dataString += " *f, ";
    
      }
//---------------------------------------------MAG MODE------------------------------------------------------------------------------------------------------
    else if (ScanMode == 3)
    {
      //Serial.println("MAG");
      dataString += "MAG: ";
      tcaselect(2); //i2c multiplexer set to BME280-9DOF
      
      if (nineDOFstart == 0) { //runs startup once the mode is selected, and only once. this speeds up bootup
        tft.setCursor(25,85);
        tft.setTextColor(ILI9341_ORANGE, ILI9341_BLACK);
        tft.setTextSize(1);
        tft.print("Starting...");
        nineDOFStartup (); nineDOFstart = 1;
        tft.print("Complete!");
      }
      
      tft.setCursor(40,170);
      tft.setTextSize(1);
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);

      nineDOFcheck();

    //------------------------------------------MAG--------------------------------------------------
      tft.setCursor(45,180); tft.print("--MAG--");
      tft.setCursor(40,193); tft.print("X: "); tft.print(myIMU.mz+465); tft.print(" <->   ");
      tft.setCursor(40,206); tft.print("Y: "); tft.print(myIMU.mx+75); tft.print(" ^/v   ");
      tft.setCursor(40,219); tft.print("Z: "); tft.print(myIMU.my+850); tft.print(" O ");tft.print("uT    ");

      tft.setCursor(25,85);
      tft.setTextColor(ILI9341_PINK, ILI9341_BLACK);
      tft.setTextSize(2);
      tft.print("Y-MAG:");tft.print(myIMU.mz); tft.print("uT  ^/v  ");

      //-------------------------------------------SD Logging ATM---------------------------------------
      int sensor1 = (myIMU.mz+465); dataString += "X axis: ";dataString += String(sensor1);dataString += " uT, ";
      int sensor2 = (myIMU.mx+75); dataString += " Y Axis: ";dataString += String(sensor2); dataString += " uT, ";
      int sensor3 = (myIMU.my+850); dataString += " Z Axis: ";dataString += String(sensor3); dataString += " uT, ";
      
    }

//---------------------------------------------MEC MODE------------------------------------------------------------------------------------------------------
    else if (ScanMode == 0)
    {//Serial.println("");
      //Serial.println("GPS");
      dataString += "MEC: ";
      tft.setTextSize(1);
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);

      nineDOFcheck();

    //------------------------------------------ACCEL------------------------------------------------
      tft.setCursor(45,125); tft.print("-ACCEL-");
      tft.setCursor(40,138); tft.print("X : "); tft.print(1000*myIMU.ax); tft.print(" G   ");
      tft.setCursor(40,151); tft.print("Y : "); tft.print(1000*myIMU.az); tft.print(" G   ");
      tft.setCursor(40,164); tft.print("Z : "); tft.print(1000*myIMU.ay); tft.print(" G   ");
        
    //------------------------------------------GYRO-------------------------------------------------
      tft.setCursor(165,180); tft.print("-GYRO-");
      tft.setCursor(160,193); tft.print("Pitch: "); tft.print(myIMU.pitch, 2); tft.print("*   ");
      tft.setCursor(160,206); tft.print("Roll : "); tft.print(myIMU.roll, 2); tft.print("*   ");
      tft.setCursor(160,219); tft.print("Yaw  : "); tft.print(myIMU.yaw, 2); tft.print("*   ");

      //-------------------------------------------SD Logging ATM---------------------------------------
      int sensor1 = (1000*myIMU.ax); dataString += " Accel X axis: ";dataString += String(sensor1);dataString += " G, ";
      int sensor2 = (1000*myIMU.az); dataString += " Accel Y Axis: ";dataString += String(sensor2); dataString += " G, ";
      int sensor3 = (1000*myIMU.ay); dataString += " Accel Z Axis: ";dataString += String(sensor3); dataString += " G, ";
       
      int sensor4 = (myIMU.pitch, 2); dataString += " Gyro Pitch: ";dataString += String(sensor4);dataString += " *rot, ";
      int sensor5 = (myIMU.roll, 2); dataString += " Gyro Roll: ";dataString += String(sensor5); dataString += " *rot, ";
      int sensor6 = (myIMU.yaw, 2); dataString += " Gyro Yaw: ";dataString += String(sensor6); dataString += " *rot, ";

    }

//---------------------------------------------DIAG MODE-----------------------------------------------------------------------------------------------------
      else if (ScanMode == 7)
      {
        if (digitalRead(A1) == LOW) { //Flashes the Color Sensor LED on and off opposing the HB LED
        digitalWrite(19, HIGH);
        } else {
        digitalWrite(19, LOW);
        }
      dataString += "DIAG: ";
      Diagscan();

      delay(500);
      playTone(1600, 10);
      }
//---------------------------------------------MODE5 RAD-----------------------------------------------------------------------------------------------------
      else if (ScanMode == 6)//cap pin 3
      {
        byte error, address;
        //Serial.println("RAD");
        dataString += "RAD: ";
        int nDevices;
        tft.setTextSize(3);
        tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK);
        tft.setCursor(25,115);tft.print("Scanning...");
        radiationWatch.loop();

        if (RadPulse == 1) {
          int sensor1 = (RadStr); int sensor2 = (RadError); dataString += " Pulse Detected: "; dataString += String(sensor1);
          dataString += " uSv/h*100 +/- "; dataString += String(RadError); RadPulse = 0; RadStr = 0; RadError = 0;}
        else {dataString += " No RAD Detection ";}
}

//--------------------------------------------GAS MODE-------------------------------------------------------------------------------------------------------
  else if (ScanMode == 2){
    //Serial.println("GAS");
    dataString += "GAS: ";

    float ratio0, ratio1, ratio2; // THIS IS ALL EVIL MAGIC, DON'T MESS WITH IT CAUSE IT'LL UNLEASH STUFF WE DON'T UNDERSTAND (don't know how this code really works lol)
    ledOn();

    int A0_0 = get_addr_dta(6, 8);int A0_1 = get_addr_dta(6, 4);int A0_2 = get_addr_dta(6, 12);
    int An_0 = get_addr_dta2(1);int An_1 = get_addr_dta2(2);int An_2 = get_addr_dta2(3);
    ratio0 = (float)An_0/(float)A0_0*(1023.0-A0_0)/(1023.0-An_0);ratio1 = (float)An_1/(float)A0_1*(1023.0-A0_1)/(1023.0-An_1);ratio2 = (float)An_2/(float)A0_2*(1023.0-A0_2)/(1023.0-An_2);
    float c = 0;

    tft.setTextSize(1);tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
    c = pow(ratio0, -1.67)/1.47;tft.setCursor(170,115);tft.print(c); int sensor1 = c;// NH3 ppm
    c = pow(ratio1, -1.179)*4.385;tft.setCursor(170,125);tft.print(c); int sensor2 = c;//CO ppm
    c = pow(ratio2, 1.007)/6.855;tft.setCursor(170,135);tft.print(c); int sensor3 = c;//NO2 ppm
    c = pow(ratio0, -2.518)*570.164;tft.setCursor(170,145);tft.print(c); int sensor4 = c;//C3H8 ppm
    c = pow(ratio0, -2.138)*398.107;tft.setCursor(170,155);tft.print(c); int sensor5 = c;//C4H10 ppm
    c = pow(ratio1, -4.363)*630.957;tft.setCursor(170,165);tft.print(c); int sensor6 = c;//CH4 ppm
    c = pow(ratio1, -1.8)*0.73;tft.setCursor(170,175);tft.print(c); int sensor7 = c;//H2 ppm
    c = pow(ratio1, -1.552)*1.622;tft.setCursor(170,185);tft.print(c); int sensor8 = c;//C2H5OH ppm
    
    ledOff();

    delay(25);

    //-------------------------------------------SD Logging GAS---------------------------------------
    dataString += " NH3 (ppm): ";dataString += String(sensor1, 3);dataString += ", ";
    dataString += " CO (ppm): ";dataString += String(sensor2, 3);dataString += ", ";
    dataString += " NO2 (ppm): ";dataString += String(sensor3, 3);dataString += ", ";
    dataString += " C3H8 (ppm): ";dataString += String(sensor4, 3);dataString += ", ";
    dataString += " C4H10 (ppm): ";dataString += String(sensor5, 3);dataString += ", ";
    dataString += " CH4 (ppm): ";dataString += String(sensor6, 3);dataString += ", ";
    dataString += " H2 (ppm): ";dataString += String(sensor7, 3);dataString += ", ";
    dataString += " C2H5OH (ppm): ";dataString += String(sensor8, 3);dataString += ", ";
    
  }
//--------------------------------------------ALT MODE-------------------------------------------------------------------------------------------------------    
  else if (ScanMode == 4)
    {
      ScanMode = OldScanMode;
      dataString += "-ALT PRESSED-";
      tft.setTextSize(2);
      tft.setCursor(268, 63);
      if (AltMode == 0){tft.setTextColor(ILI9341_BLACK, ILI9341_WHITE);tft.print("ALTM");AltMode=1;}
      else if (AltMode == 1){tft.setTextColor(ILI9341_BLACK, ILI9341_WHITE);tft.print("NORM");AltMode=0;}
      playTone(1850, 40);
      leds[0] = CRGB(30,30,30); FastLED.show();//colorLED
      delay(250);
      leds[0] = CRGB(0,0,0); FastLED.show();//colorLED
    }
      
//---------------------------------------------LOOP end------------------------------------------------------------------------------------------------------  
  else { }
 ButtonAction ();
 
 int tmpCounter = 0;

  //-------------------------SD Write------------------------------------
  //SD Logfile check and dump
  // open the file. note that only one file can be open at a time,
  // so you have to close this one before opening another.
  File dataFile = SD.open("datalog.txt", FILE_WRITE);
  //Serial.println("SD File Opened");

  // if the file is available, write to it:
  if (dataFile) {
    dataFile.println(dataString);
    dataFile.close();
    // print to the serial port too:
    Serial.println(dataString);
  }
    // if the file isn't open, pop up an error:
  else {
    Serial.println("error opening datalog.txt");
  }
  //----------------------------------------------------------------------

 while (digitalRead(5) == FALLING){ //Cap Interrupt
   if( (millis() - (digitalRead(5) == FALLING)) > 6000 || inReset){
     if( inReset )
       inReset = false;
       leds[0] = CRGB(30,30,30); FastLED.show();
       if (ScanMode != 2){BlankDataReadout();}
     break;
   } else {
   }
  }

  if (digitalRead(A1) == HIGH) { //HB LED On/off
  digitalWrite(A1, LOW);
  } else {
  digitalWrite(A1, HIGH);
  }
  //---------------------------------------------DEBUG---------------------------------------------------------------------------------------------------------

  //Showing the ButtonAct variable
      tft.setTextSize(2);
      tft.setCursor(232, 63);
      if (ButtonAct == 1){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("LFT");}      //mec
      else if (ButtonAct == 2){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("-O-");} //atm
      else if (ButtonAct == 3){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("-UP");} //gas
      else if (ButtonAct == 4){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("DWN");} //mag
      else if (ButtonAct == 5){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("-X-");} //lum
      else if (ButtonAct == 6){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("RGT");} //rad
      else if (ButtonAct == 7){tft.setTextColor(ILI9341_BLACK, ILI9341_YELLOW);tft.print("BCK");} //diag
      else if (ButtonAct == 0){tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);tft.print("NLL");} //n/a, bootup only











  
}
